import math
import numpy as np
import multiprocessing
import yaml
import time
import queue
from yolov5.detect import *
from serial_port import Serial_port
from systemControl import SystemControl

hym_arg = argparse.ArgumentParser()
hym_arg.add_argument('--yaml', type=str, default='config/pc.yaml', help=argparse.SUPPRESS)
hym_opt = hym_arg.parse_args()
with open(hym_opt.yaml, 'r', encoding='utf-8') as f:
    hyp = yaml.load(f.read(), Loader=yaml.SafeLoader)

stop_flag = {}
self_test_flag = {}
manipulation_flags = {}
for key in hyp.keys():
    if hyp[key]['open']:
        manipulation_flags[key] = hyp[key]['manipulation_flags']
        stop_flag[key] = hyp[key]['initial_stop_state']
        self_test_flag[key] = hyp[key]['self_test_state']
systemcontrol = SystemControl()


class Laser_ranging():
    """激光测距"""

    def __init__(self, port, baudrate, bytesize=8, stopbits=1, timeout=0.8, parity='N'):
        self.port = port
        self.baudrate = baudrate
        self.bytesize = bytesize
        self.stopbits = stopbits
        self.timeout = timeout
        self.parity = parity
        self.Laser = Serial_port(self.port,
                                 self.baudrate,
                                 self.bytesize,
                                 self.stopbits,
                                 self.timeout,
                                 self.parity)

    def Laser_ranging_on(self):
        """
        开启激光
        由产品手册得知，往模块发送大写字母O即可开启激光
        """
        self.Laser.send(send_data='iLD:1')
        data = self.Laser.receive(read_len=20)
        if 'OPEN' in data:  # 开启成功返回True
            return True
        else:
            return False

    def Laser_ranging_off(self):
        """
        关闭激光
        由产品手册得知，往模块发送大写字母C即可开启激光
        """
        self.Laser.send(send_data='iLD:0')
        data = self.Laser.receive(read_len=20)
        if 'CLOSE' in data:  # 关闭成功返回True
            return True
        else:
            return False

    def Laser_ranging_measure_off(self):
        self.Laser.send(send_data='iHALT')
        data = self.Laser.receive(read_len=20)
        if 'STOP' in data:
            return True
        else:
            return False

    def Laser_ranging_measure_on(self, mode):
        if mode == "standard":
            self.Laser.send(send_data='iSM')
        elif mode == "slow":
            self.Laser.send(send_data='iACM')
        elif mode == "fast":
            self.Laser.send(send_data='iFACM')

    def Laser_ranging_measure(self):
        """
        测量距离，入口参数：串口实例，测量模式
        由产品手册得知，测量模式为：D->标准模式，M->慢速模式，F->快速模式
        由产品手册得知，读取从串口返回数据为：'12.345m,0079'字符串类型
        Args:
            mode: 模式

        Returns: 距离，准确度

        """
        distance = 0  # 距离定义

        if self.Laser.com.in_waiting:
            data = self.Laser.receive(read_len=20)  # 得到返回字符串
            distance = data.split('D')[1][1:6]  # 处理得到距离，单位m

        return distance


class Control_board():
    def __init__(self, port, baudrate, bytesize=8, stopbits=1, timeout=0.01, parity='N'):
        self.port = port
        self.baudrate = baudrate
        self.bytesize = bytesize
        self.stopbits = stopbits
        self.timeout = timeout
        self.parity = parity
        self.mpu = Serial_port(self.port,
                               self.baudrate,
                               self.bytesize,
                               self.stopbits,
                               self.timeout,
                               self.parity)

        # 相机中心点坐标

    def send_message(self, data, key, pad_len=6):
        data = '#{}e'.format(self.combine_data(data, pad_len))
        print(f"Process: {'%s %s' % (key, multiprocessing.current_process().name):-<20}send message: {data}")
        self.mpu.send(send_data=data, )

    def coordinate_difference(self, data_tuple, translation_vector=[0, 0], capture_size=[640, 480]):
        """
        取出离像素中点最近的目标物中心点
        Args:
            data_tuple:
            translation_vector:
            capture_size:

        Returns:

        """
        center_x, center_y = capture_size[0] / 2 + translation_vector[0], capture_size[1] / 2 + translation_vector[1]
        abs_min_dx, abs_min_dy = 100000, 100000
        min_dx, min_dy = 0, 0
        for x, y in zip(data_tuple[0], data_tuple[1]):
            if abs(x - center_x) < abs_min_dx:
                abs_min_dx = abs(x - center_x)
                min_dx = x - center_x
            if abs(y - center_y) < abs_min_dy:
                abs_min_dy = abs(y - center_y)
                min_dy = y - center_y
        return [min_dx], [min_dy]

    def combine_data(self, data_tuple, pad_len):
        """
        输入两个列表组成的一个元组
        Args:
            data_tuple:形如 ([1,1,1], [1,1,1])

        Returns:

        """
        send_str = ''
        for A, B in zip(data_tuple[0], data_tuple[1]):
            # 角度数据一个目标的数据包含12位，偏航角和俯仰角坐标各5位（+00.0），# _ 占两位，例子：#+11.0_-11.1
            # 坐标差数据一个目标的数据包含14位，横纵坐标差坐标各6位（+000.0），# _ 占两位，例子：#+111.0_-111.1e或者00#+11.1_-11.1
            send_str = f"{A:+0{pad_len}.1f}_{B:+0{pad_len}.1f}"
        return send_str

    def send_flag_frame(self, is_stop, key):
        data = f"#{int(is_stop)}e"
        print(f"Process: {'%s %s' % (key, multiprocessing.current_process().name):-<20}send message: {data}")
        self.mpu.send(send_data=data)


def stop_message(control_board, key):
    global stop_flag, self_test_flag
    stop_id = control_board.mpu.receive()
    # print(f"{key}: {stop_id}")
    control_board.mpu.com.flushInput()  # 清空缓存区数据
    if stop_id == manipulation_flags[key]['stop']:
        stop_flag[key] = True  # 进程停止
    elif stop_id == manipulation_flags[key]['start']:
        stop_flag[key] = False  # 进程开启
    elif stop_id == manipulation_flags[key]['self_test']:
        self_test_flag[key] = False  # 自检关闭
    elif stop_id == manipulation_flags[key]['shutdown']:
        systemcontrol.shutDown()  # 关机
    elif stop_id == manipulation_flags[key]['reboot']:
        systemcontrol.reboot()  # 重启
    return stop_flag[key], self_test_flag[key]


class GpioControl():
    def __init__(self, pin):
        super(GpioControl, self).__init__()
        import RPi.GPIO as GPIO
        self.pin = pin
        self.gpio = GPIO
        self.gpio.setmode(GPIO.BCM)
        self.gpio.setup(pin, GPIO.OUT)

    def flicker(self, delay=0.01):
        self.gpio.output(self.pin, self.gpio.HIGH)
        time.sleep(delay)
        self.gpio.output(self.pin, self.gpio.LOW)

    def output_high(self):
        self.gpio.output(self.pin, self.gpio.HIGH)

    def output_low(self):
        self.gpio.output(self.pin, self.gpio.LOW)

    def gpio_close(self):
        print(f"pin{self.pin} clean")
        self.gpio.cleanup()


class Yolov5():
    def __init__(self, weights_path, source, data, img_size, cap_info, view_img, conf_thres, iou_thres, nosave, dnn,
                 camera_matrix, dist_coeffs):
        """
        yolov5 目标检测
        Args:
            weights_path: 模型地址
            source: 图片或视频
            data: 配置参数地址
            img_size: 推理图片大小
            view_img: 是否可视化
            nosave: 是否保存结果
        """
        super(Yolov5, self).__init__()
        self.weights_path = weights_path
        self.source = source
        self.data = data
        self.img_size = img_size
        self.cap_info = cap_info
        self.view_img = view_img
        self.conf_thres = conf_thres
        self.iou_thres = iou_thres
        self.nosave = nosave
        self.dnn = dnn
        self.camera_matrix = camera_matrix
        self.dist_coeffs = dist_coeffs

    def run_detect(self, queen, queen_stop=None):
        """
        开启目标检测
        Args:
            queen:
            queen_stop: 队列的实例

        Returns:无

        """
        self.opt = self.parse_opt(self.weights_path,
                                  self.source,
                                  self.data,
                                  self.img_size,
                                  self.cap_info,
                                  self.view_img,
                                  self.nosave,
                                  self.conf_thres,
                                  self.iou_thres,
                                  self.dnn,
                                  self.camera_matrix,
                                  self.dist_coeffs,
                                  queen,
                                  queen_stop)
        main(self.opt)

    def parse_opt(self, weights, source, data, img_size, cap_info, view_img,
                  nosave, conf_thres=0.25, iou_thres=0.45, dnn=False,
                  camera_matrix=[], dist_coeffs=[], queen=None, queen_stop=None):
        """
        配置初始化参数
        Args:

            conf_thres:
            iou_thres:
            dnn:
            queen:
            dist_coeffs:
            camera_matrix:
            weights:
            source:
            data:
            img_size:
            view_img:
            nosave:
            conn:
            queen_stop:

        Returns: 配置结果

        """
        parser = argparse.ArgumentParser()
        parser.add_argument('--weights', nargs='+', type=str, default=weights,
                            help='model path or triton URL')
        parser.add_argument('--source', type=str, default=source,
                            help='file/dir/URL/glob/screen/0(webcam)')
        parser.add_argument('--data', type=str, default=data, help='(optional) dataset.yaml path')
        parser.add_argument('--imgsz', '--img', '--img-size', nargs='+', type=int, default=img_size,
                            help='inference size h,w')
        parser.add_argument('--cap_info', '--cap', nargs='+', type=int, default=cap_info,
                            help='capture size h,w')
        parser.add_argument('--conf-thres', type=float, default=conf_thres, help='confidence threshold')
        parser.add_argument('--iou-thres', type=float, default=iou_thres, help='NMS IoU threshold')
        parser.add_argument('--max-det', type=int, default=1000, help='maximum detections per image')
        parser.add_argument('--device', default='', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
        parser.add_argument('--view-img', type=bool, default=view_img, help='show results')
        parser.add_argument('--save-txt', action='store_true', help='save results to *.txt')
        parser.add_argument('--save-conf', action='store_true', help='save confidences in --save-txt labels')
        parser.add_argument('--save-crop', action='store_true', help='save cropped prediction boxes')
        parser.add_argument('--nosave', type=bool, default=nosave, help='do not save images/videos')
        parser.add_argument('--classes', nargs='+', type=int, help='filter by class: --classes 0, or --classes 0 2 3')
        parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
        parser.add_argument('--augment', action='store_true', help='augmented inference')
        parser.add_argument('--visualize', action='store_true', help='visualize features')
        parser.add_argument('--update', action='store_true', help='update all models')
        parser.add_argument('--project', default=ROOT / 'runs/detect', help='save results to project/name')
        parser.add_argument('--name', default='exp', help='save results to project/name')
        parser.add_argument('--exist-ok', action='store_true', help='existing project/name ok, do not increment')
        parser.add_argument('--line-thickness', default=3, type=int, help='bounding box thickness (pixels)')
        parser.add_argument('--hide-labels', default=False, action='store_true', help='hide labels')
        parser.add_argument('--hide-conf', default=False, action='store_true', help='hide confidences')
        parser.add_argument('--half', action='store_true', help='use FP16 half-precision inference')
        parser.add_argument('--dnn', type=bool, default=dnn, help='use OpenCV DNN for ONNX inference')
        parser.add_argument('--vid-stride', type=int, default=1, help='video frame-rate stride')
        parser.add_argument('--camera_matrix', default=camera_matrix, help='camera_matrix')
        parser.add_argument('--dist_coeffs', default=dist_coeffs, help='dist_coeffs')
        parser.add_argument('--queen', default=queen, help='video frame-rate stride')
        parser.add_argument('--queen_stop', default=queen_stop)
        parser.add_argument('--yaml')
        opt = parser.parse_args()
        opt.imgsz *= 2 if len(opt.imgsz) == 1 else 1  # expand
        opt.cap_info['size'] *= 2 if len(opt.cap_info['size']) == 1 else 1
        print_args(vars(opt))
        return opt
